/**
 * Dual stepper motor driver
 * @author Simon Richards
 * @date 16/02/2011
 *
 * Status: Untested, uncompiled
 */
#include "common.h"
#define NUM_MOTORS 2

static int left_period = 0;
static int right_period = 0;

enum {A,AB,B,BC,C,CD,DA};

enum {LEFT, RIGHT};

static uint8_t left_states[] = {
    SET_LEFT_A,
    SET_LEFT_AB,
    SET_LEFT_B,
    SET_LEFT_BC,
    SET_LEFT_C,
    SET_LEFT_CD,
    SET_LEFT_D,
    SET_LEFT_DA,
}

static uint8_t right_states[] = {
    SET_RIGHT_A,
    SET_RIGHT_AB,
    SET_RIGHT_B,
    SET_RIGHT_BC,
    SET_RIGHT_C,
    SET_RIGHT_CD,
    SET_RIGHT_D,
    SET_RIGHT_DA,
}

void motor_init(void)
{
    P1DIR |= 0xff;
}

/**
 * Call at a fixed frequency
 */
void motor_update(void)
{
    static int left_count = 0;
    static int right_count = 0;
    if (left_period && left_count++ > left_period)
    {
        motor_update_left();
        left_count = 0;
    }
    if (right_period && right_count++ > right_period)
    {
        motor_update_right();
        right_count = 0;
    }
}


/**
 * Dumb update routines which simply switche to the
 * next state. Call at correct frequency to
 * implement speed control.
 */
void motor_update_right(void)
{
    static int state = A;
    P1OUT &= ~right_states[state];
    state = (state == SET_LEFT_DA) ? SET_LEFT_A : state + 1;
    P1OUT |= right_states[state];
}

void motor_update_left(void)
{
    static int state = A;
    P1OUT &= ~left_states[state];
    state = (state == SET_LEFT_DA) ? SET_LEFT_A : state + 1;
    P1OUT |= left_states[state];
}

/**
 * Use this to set either motor's speed
 *
 * @param id the motor's name (LEFT or RIGHT)
 * @param period the period between motor steps
 */
void motor_set(int id, int period)
{
    if (id == LEFT)
       left_period = period;
    else // (id == RIGHT)
       right_period = period;
}
